
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       rc.c
  * @author     baiyang
  * @date       2021-8-8
  ******************************************************************************
  */

/*----------------------------------include-----------------------------------*/
#include "rc.h"

#if defined(HAL_WITH_IO_MCU)
#include <iomcu/gp_iomcu.h>
#endif

#include <uITC/uITC_msg.h>
#include <common/microbee.h>
#include <gcs_mavlink/gcs.h>
#include <common/time/gp_time.h>
#include <rc_protocol/rc_protocol.h>
#include <board_config/borad_config.h>
#include <common/gp_math/gp_mathlib.h>

#if defined(HAL_USE_EICU)
#include <drivers/soft_sigreader_int.h>
#endif
/*-----------------------------------macro------------------------------------*/
#ifndef RC_HAL_GCS_SEND_TEXT
#define RC_HAL_GCS_SEND_TEXT(severity, format, ...) gcs_send_text(severity, format, ##__VA_ARGS__)
#endif
/*----------------------------------typedef-----------------------------------*/

/*---------------------------------prototype----------------------------------*/
static void rc_hal_timer_tick(void);
/*----------------------------------variable----------------------------------*/
rc_hal_mgr rc_hal;

static char thread_rcin_stack[1024*2];
static struct rt_thread thread_rcin_handle;
/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
static void rc_hal_entry(void *parameter)
{
    while(1)
    {
        rt_thread_mdelay(1);
        rc_hal_timer_tick();
    }
}

void rc_hal_ctor()
{
    rc_hal._rssi = -1;
    rc_hal._rx_link_quality = -1;
}

void rc_hal_init()
{
    rt_err_t res;

    rcprotocol_ctor();
    rcprotocol_init();

    rt_mutex_init(&rc_hal.rcin_mutex, "rcin", RT_IPC_FLAG_FIFO);

    res = rt_thread_init(&thread_rcin_handle,
                           "rcin",
                           rc_hal_entry,
                           RT_NULL,
                           &thread_rcin_stack[0],
                           sizeof(thread_rcin_stack),PRIORITY_RCIN,2);

    RT_ASSERT(res == RT_EOK);

    if (res == RT_EOK) {
        rt_thread_startup(&thread_rcin_handle);
    }

#if defined(HAL_USE_EICU)
    ssi_init();
    rc_hal.pulse_input_enabled = true;
#endif

    rc_hal._init = true;
}

/*
  enable or disable pulse input for RC input. This is used to reduce
  load when we are decoding R/C via a UART
*/
void rc_hal_pulse_input_enable(bool enable)
{
    rc_hal.pulse_input_enabled = enable;
}

bool rc_hal_new_input()
{
    if (!rc_hal._init) {
        return false;
    }
    bool valid;

    rt_mutex_take(&rc_hal.rcin_mutex, RT_WAITING_FOREVER);

    valid = rc_hal._rcin_timestamp_last_signal != rc_hal._last_read;
    rc_hal._last_read = rc_hal._rcin_timestamp_last_signal;

    rt_mutex_release(&rc_hal.rcin_mutex);

    return valid;
}

uint8_t rc_hal_num_channels()
{
    if (!rc_hal._init) {
        return 0;
    }
    return rc_hal._num_channels;
}

uint16_t rc_hal_read(uint8_t channel)
{
    if (!rc_hal._init || (channel >= MIN(RC_INPUT_MAX_CHANNELS, rc_hal._num_channels))) {
        return 0;
    }
    uint16_t v;

    rt_mutex_take(&rc_hal.rcin_mutex, RT_WAITING_FOREVER);

    v = rc_hal._rc_values[channel];

    rt_mutex_release(&rc_hal.rcin_mutex);

    return v;
}

uint8_t rc_hal_read2(uint16_t* periods, uint8_t len)
{
    if (!rc_hal._init) {
        return false;
    }

    if (len > RC_INPUT_MAX_CHANNELS) {
        len = RC_INPUT_MAX_CHANNELS;
    }

    rt_mutex_take(&rc_hal.rcin_mutex, RT_WAITING_FOREVER);

    rt_memcpy(periods, rc_hal._rc_values, len*sizeof(periods[0]));

    rt_mutex_release(&rc_hal.rcin_mutex);

    return len;
}

/*
  start a bind operation, if supported
 */
bool rc_hal_rc_bind(int dsmMode)
{
#if defined(HAL_WITH_IO_MCU)
    rt_mutex_take(&rc_hal.rcin_mutex, RT_WAITING_FOREVER);

    if (brd_io_enabled()) {
        iomcu_bind_dsm(dsmMode);
    }

    rt_mutex_release(&rc_hal.rcin_mutex);
#endif

    return true;
}

static void rc_hal_timer_tick(void)
{
    if (!rc_hal._init) {
        return;
    }
#ifndef HAL_NO_UARTDRIVER
    const char *rc_protocol = NULL;
    enum RCSource source = rc_hal.last_source;
#endif

#if defined(HAL_USE_EICU)
    if (rc_hal.pulse_input_enabled) {
        uint32_t width_s0, width_s1;
        while(ssi_read(&width_s0, &width_s1)) {
            rcprotocol_process_pulse(width_s0, width_s1);
        }
    }
#endif

    uint32_t now = time_millis();
    const bool have_iocmu_rc = (rc_hal._rcin_last_iomcu_ms != 0 && now - rc_hal._rcin_last_iomcu_ms < 400);
    if (!have_iocmu_rc) {
        rc_hal._rcin_last_iomcu_ms = 0;
    }

    if (rcprotocol_new_input() && !have_iocmu_rc) {
        rt_mutex_take(&rc_hal.rcin_mutex, RT_WAITING_FOREVER);
        rc_hal._rcin_timestamp_last_signal = time_micros();
        rc_hal._num_channels = rcprotocol_num_channels();
        rc_hal._num_channels = MIN(rc_hal._num_channels, RC_INPUT_MAX_CHANNELS);
        rcprotocol_read2(rc_hal._rc_values, rc_hal._num_channels);
        rc_hal._rssi = rcprotocol_get_RSSI();
        rc_hal._rx_link_quality = rcprotocol_get_rx_link_quality();
#ifndef HAL_NO_UARTDRIVER
        rc_protocol = rcprotocol_protocol_name();
        source = rcprotocol_using_uart() ? RCSOURCE_RCPROT_BYTES : RCSOURCE_RCPROT_PULSES;
#endif
        rt_mutex_release(&rc_hal.rcin_mutex);
    }

#if defined(HAL_WITH_IO_MCU)
    rt_mutex_take(&rc_hal.rcin_mutex, RT_WAITING_FOREVER);

    if (brd_io_enabled() &&
        iomcu_check_rcinput(&rc_hal.last_iomcu_us, &rc_hal._num_channels, rc_hal._rc_values, RC_INPUT_MAX_CHANNELS)) {
        rc_hal._rcin_timestamp_last_signal = (uint32_t)rc_hal.last_iomcu_us;
        rc_hal._rcin_last_iomcu_ms = now;
#ifndef HAL_NO_UARTDRIVER
        rc_protocol = iomcu_get_rc_protocol();
        rc_hal._rssi = iomcu_get_RSSI();
        source = RCSOURCE_IOMCU;
#endif
    }

    rt_mutex_release(&rc_hal.rcin_mutex);
#endif

#ifndef HAL_NO_UARTDRIVER
    if (rc_protocol && rc_protocol != rc_hal.last_protocol) {
        rc_hal.last_protocol = rc_protocol;
        rc_hal.last_source = source;
        RC_HAL_GCS_SEND_TEXT(MAV_SEVERITY_DEBUG, "RCInput: decoding %s(%u)", rc_hal.last_protocol, (unsigned)(source));
    }
#endif

    // note, we rely on the vehicle code checking new_input()
    // and a timeout for the last valid input to handle failsafe
}

/*------------------------------------test------------------------------------*/


